home *** CD-ROM | disk | FTP | other *** search
/ The 640 MEG Shareware Studio 2 / The 640 Meg Shareware Studio CD-ROM Volume II (Data Express)(1993).ISO / clang / jcool01.zip / QUATERNI.C < prev    next >
C/C++ Source or Header  |  1992-10-01  |  10KB  |  259 lines
  1. //
  2. // Copyright (C) 1992 General Electric Company.
  3. //
  4. // Permission is granted to any individual or institution to use, copy, modify,
  5. // and distribute this software, provided that this complete copyright and
  6. // permission notice is maintained, intact, in all copies and supporting
  7. // documentation.
  8. //
  9. // General Electric Company,
  10. // provides this software "as is" without express or implied warranty.
  11. //
  12. // Created: VDN 06/23/92 -- design and implementation
  13. //
  14. // Rep Invariant: 
  15. //   1. norm = 1, for a rotation.
  16. //   2. position vector represented by imaginary quaternion.
  17.  
  18.  
  19. #include <cool/Quaternion.h>
  20.  
  21. #include <cool/Matrix.C>
  22. #include <cool/M_Vector.C>
  23.  
  24. #define CoolEnvelope CoolEnvelope_Quaternion
  25.  
  26.  
  27. // Quaternion -- Construct Quaternion from the 4 components
  28. // Input:      3 imaginary and 1 real components
  29. // Ouput:      none.
  30.  
  31. CoolQuaternion::CoolQuaternion (float x, float y, float z, float r) 
  32. : CoolM_Vector<float>(4) {
  33.   this->data[0] = x;                // 3 first elmts are 
  34.   this->data[1] = y;                // imaginary parts
  35.   this->data[2] = z;
  36.   this->data[3] = r;                // last element is real part
  37. }  
  38.  
  39. // Quaternion -- Construct Quaternion from angle and axis of rotation.
  40. // Input:      angle in radians, and 3D normalized vector for axis of rotation.
  41. // Ouput:      none.
  42.  
  43. CoolQuaternion::CoolQuaternion (const CoolM_Vector<float>& axis, float angle)
  44. : CoolM_Vector<float>(4) {
  45.   double a = angle / 2.0;            // half angle
  46.   double s = sin(a);
  47.   CoolM_Vector<float>& axis2 = (CoolM_Vector<float>&) axis; // cast away const
  48.   for (int i = 0; i < 3; i++)            // imaginary vector is sin of
  49.     this->data[i] = s * axis2(i);        // half angle multiplied with axis
  50.   this->data[3] = cos(a);            // real part is cos of half angle
  51. }
  52.  
  53.  
  54. // should be part of transform class.
  55.  
  56. CoolEnvelope_Matrix/*##*/< CoolMatrix<float> > extract_3d_rotation (const CoolMatrix<float>& transform) {
  57.   if (transform.rows() != transform.columns()) {
  58.     printf("Ambiguous rotation submatrix from a %dx%d transform.\n", 
  59.        transform.rows(), transform.columns());
  60.     abort();
  61.   }
  62.   if (transform.rows() >= 3)
  63.     return transform.extract(3, 3, 0, 0);    // rotation is top-left most block
  64.   else {
  65.     CoolMatrix<float> rot(3, 3, 0.0);
  66.     CoolMatrix<float>& transform2 = (CoolMatrix<float>&) transform;
  67.     for (int i = 0; i < 2; i++)            // extract rotation in xy-plane
  68.       for (int j = 0; j < 2; j++)        // from transform.
  69.     rot(i,j) = transform2(i,j);
  70.     rot(2,2) = 1.0;                // zz component 
  71.     CoolEnvelope_Matrix/*##*/< CoolMatrix<float> >& result = *((CoolEnvelope_Matrix< CoolMatrix<float> >*) &rot);
  72.     return result;                // avoid deep copy with envelope
  73.   }
  74. }
  75.  
  76. // Quaternion -- Construct Quaternion from 2-4 square row-major transform.
  77. //              Basis vectors is stored row-wise in submatrix(3,3,0,0).
  78. // Input:       Transformation matrix, with orthonormal basis vectors.
  79. // Output:      none.
  80.  
  81. CoolQuaternion::CoolQuaternion (const CoolMatrix<float>& transform) 
  82. : CoolM_Vector<float>(4) {
  83.   CoolMatrix<float> rot = extract_3d_rotation(transform);
  84.   double d0 = rot(0,0), d1 = rot(1,1), d2 = rot(2,2);
  85.   double xx = 1.0 + d0 - d1 - d2;        // from the diagonal of rotation
  86.   double yy = 1.0 - d0 + d1 - d2;        // matrix, find the terms in
  87.   double zz = 1.0 - d0 - d1 + d2;        // each Quaternion compoment
  88.   double rr = 1.0 + d0 + d1 + d2;
  89.  
  90.   double max = rr;                // find the maximum of all
  91.   if (xx > max) max = xx;                // diagonal terms.
  92.   if (yy > max) max = yy;
  93.   if (zz > max) max = zz;
  94.  
  95.   if (rr == max) {
  96.     double r4 = sqrt(rr * 4.0);
  97.     this->x() = (rot(1,2) - rot(2,1)) / r4;    // find other components from
  98.     this->y() = (rot(2,0) - rot(0,2)) / r4;    // off diagonal terms of
  99.     this->z() = (rot(0,1) - rot(1,0)) / r4;    // rotation matrix.
  100.     this->r() = r4 / 4.0;
  101.   } else if (xx == max) {
  102.     double x4 = sqrt(xx * 4.0);
  103.     this->x() = x4 / 4.0;
  104.     this->y() = (rot(0,1) + rot(1,0)) / x4;
  105.     this->z() = (rot(0,2) + rot(2,0)) / x4;
  106.     this->r() = (rot(1,2) - rot(2,1)) / x4;
  107.   } else if (yy == max) {
  108.     double y4 = sqrt(yy * 4.0);
  109.     this->x() = (rot(0,1) + rot(1,0)) / y4;
  110.     this->y() =  y4 / 4.0;
  111.     this->z() = (rot(1,2) + rot(2,1)) / y4;
  112.     this->r() = (rot(2,0) - rot(0,2)) / y4;
  113.   } else {
  114.     double z4 = sqrt(zz * 4.0);
  115.     this->x() = (rot(0,2) + rot(2,0)) / z4;
  116.     this->y() = (rot(1,2) + rot(2,1)) / z4;
  117.     this->z() =  z4 / 4.0;
  118.     this->r() = (rot(0,1) - rot(1,0)) / z4;
  119.   }
  120. }
  121.  
  122. // angle -- Positive angle of rotation
  123.  
  124. float CoolQuaternion::angle () const {
  125.   double sin = 0;
  126.   for (int i = 0; i < 3; i++)            // compute sin of half angle
  127.     sin += this->data[i] * this->data[i];    // from imaginary vector
  128.   sin = sqrt(sin);
  129.   double cos = this->data[3];            // cos of half angle
  130.   return (2.0 * atan2 (sin, cos));        // angle is always positive
  131. }
  132.  
  133. // axis -- Normalized direction vector of axis of rotation.
  134. //         Returns (0, 0, 1) if Quaternion is zero, and axis is not well defined.
  135.  
  136. CoolEnvelope_M_Vector/*##*/< CoolM_Vector<float> > CoolQuaternion::axis () const {
  137.   CoolM_Vector<float> dir = this->imaginary(); // dir parallel to imag. part
  138.   float mag = dir.magnitude();
  139.   if (mag == 0) {
  140.     cout << "Axis is not well defined for zero Quaternion. Use (0,0,1) instead. "
  141.       << endl;
  142.     dir.z() = 1.0;                // or signal exception here.
  143.   } else 
  144.     dir /= mag;                    // normalize direction vector
  145.   CoolEnvelope_M_Vector/*##*/< CoolM_Vector<float> >& result = *((CoolEnvelope_M_Vector/*##*/< CoolM_Vector<float> >*) &dir);
  146.   return result;                // avoid deep copy
  147. }
  148.  
  149.  
  150. // operator== -- Components of Quaternion are compared with fuzz = 1.0e-6
  151.  
  152. Boolean CoolQuaternion::operator== (const CoolQuaternion& rhs) const {
  153.   for (int i = 0; i < 4; i++)
  154.     if (fabs(this->data[i] - rhs.data[i]) > 1.0e-6) // more fuzz because of
  155.       return FALSE;                    // sqrt, etc...
  156.   return TRUE;
  157. }
  158.  
  159.  
  160. // rotation_transform -- Convert to a rotation transform matrix with 
  161. //            specified rows & cols. Assumed normalized Quaternion.
  162. // Input:     number of rows and columns specifying format of transform.
  163. // Output:    matrix for rotation transform equivalent to Quaternion.
  164.  
  165. CoolEnvelope_Matrix/*##*/< CoolMatrix<float> > CoolQuaternion::rotation_transform (int dim) const {
  166.   CoolMatrix<float> rot(dim, dim, 0.0);
  167.   CoolQuaternion q(*this);            
  168.   if (dim == 2) {
  169.     q.x() = q.y() = 0.0;            // find best approx rotation
  170.     q.normalize();                // along z-axis only.
  171.     double s = q.z(), c = q.r();
  172.     rot(0,0) = rot(1,1) = (c * c) - (s * s);
  173.     rot(0,1) = 2.0 * s * c;
  174.     rot(1,0) = -rot(0,1);
  175.   } 
  176.   if (dim >= 3) {
  177.     double x2 = q.x() * q.x();
  178.     double y2 = q.y() * q.y();
  179.     double z2 = q.z() * q.z();
  180.     double r2 = q.r() * q.r();
  181.     rot(0,0) = r2 + x2 - y2 - z2;        // fill diagonal terms
  182.     rot(1,1) = r2 - x2 + y2 - z2;
  183.     rot(2,2) = r2 - x2 - y2 + z2;
  184.     double xy = q.x() * q.y();
  185.     double yz = q.y() * q.z();
  186.     double zx = q.z() * q.x();
  187.     double rx = q.r() * q.x();
  188.     double ry = q.r() * q.y();
  189.     double rz = q.r() * q.z();
  190.     rot(0,1) = 2 * (xy + rz);            // fill off diagonal terms
  191.     rot(0,2) = 2 * (zx - ry);
  192.     rot(1,2) = 2 * (yz + rx);
  193.     rot(1,0) = 2 * (xy - rz);
  194.     rot(2,0) = 2 * (zx + ry);
  195.     rot(2,1) = 2 * (yz - rx);
  196.   }
  197.   if (dim == 4) rot(3,3) = 1.0;            // for homogeneous transform
  198.   CoolEnvelope_Matrix/*##*/< CoolMatrix<float> >& result = *((CoolEnvelope_Matrix/*##*/< CoolMatrix<float> >*) &rot);
  199.   return result;                // avoid deep copy with envelope
  200. }
  201.  
  202. // conjugate -- Conjugate of a Quaternion has same real part and 
  203. //            opposite imaginary part.
  204.  
  205. CoolEnvelope<CoolQuaternion> CoolQuaternion::conjugate () const {
  206.   CoolQuaternion* self = (CoolQuaternion*) this;    // cast away const
  207.   CoolQuaternion conj(-self->x(), -self->y(), -self->z(), self->r());
  208.   CoolEnvelope<CoolQuaternion>& result = *((CoolEnvelope<CoolQuaternion> *) &conj); // avoid deep copy with
  209.   return result;                  // envelope
  210. }
  211.  
  212. // inverse -- Inverse Quaternion exists only for nonzero Quaternion.
  213. //            If Quaternion represents rotation, inverse is conjugate.
  214.  
  215. CoolEnvelope<CoolQuaternion> CoolQuaternion::inverse () const {
  216.   CoolQuaternion inv = this->conjugate() / dot_product(*this, *this);
  217.   CoolEnvelope<CoolQuaternion>& result = *((CoolEnvelope<CoolQuaternion> *) &inv); // avoid deep copy with
  218.   return result;                  // envelope
  219. }
  220.  
  221. // operator* -- Multiplication of two Quaternions is not symmetric
  222. //            and has fewer operations than mult of orthonormal matrices 
  223. //            If object is rotated by r1, then by r2, then the composed
  224. //            rotation (r2 o r1) is represented by the Quaternion (q2 * q1),
  225. //            and by the matrix (m1 * m2). 
  226. //            Remember that matrix and vector are represented row-wise,
  227. //            and this reverses the composition order.
  228.  
  229. CoolEnvelope<CoolQuaternion> operator* (const CoolQuaternion& q1, const CoolQuaternion& q2) {
  230.   float r1 = q1.real();                // real and img parts of args
  231.   float r2 = q2.real();        
  232.   CoolM_Vector<float> i1 = q1.imaginary();
  233.   CoolM_Vector<float> i2 = q2.imaginary();
  234.   float real = (r1 * r2) - dot_product(i1, i2);    // real&img of product
  235.   CoolM_Vector<float> img = (r1 * i2) + (r2 * i1) + cross_3d(i1, i2);
  236.   CoolQuaternion prod(img.x(), img.y(), img.z(), real); 
  237.   CoolEnvelope<CoolQuaternion>& result = *((CoolEnvelope<CoolQuaternion> *) &prod); // avoid deep copy with
  238.   return result;                  // envelope
  239. }
  240.  
  241. // rotate --  Transform 3D vector by rotation Quaternion
  242. //            
  243.  
  244. void CoolQuaternion::rotate (CoolM_Vector<float>& v) const {
  245.   float r = this->real();
  246.   CoolM_Vector<float> i = this->imaginary();
  247.   v += float(2 * r) * cross_3d(i, v) -        // fewer number of mults
  248.     float(2) * cross_3d(cross_3d(i, v), i);
  249. }
  250.  
  251. //### {Jam}This was not originally in the code, but I had to add it because
  252. // BC++ 3.1 said Envelope needed it.
  253. inline void operator*=(CoolQuaternion& q1, const CoolQuaternion& q2)
  254.  { q1 = q1 * q2; }
  255.  
  256.  
  257. #undef CoolEnvelope
  258.  
  259.